#include <stdio.h>
#include <sys/time.h>
#include <emmintrin.h>

#define N 1000
#define SM (CLS / sizeof (double))

int i, i2, j, j2, k, k2;

// compiler aligns arrays to 64bytes
double res[N][N] __attribute__ ((aligned (64)));
double mul1[N][N] __attribute__ ((aligned (64)));
double mul2[N][N] __attribute__ ((aligned (64)));
double tmp[N][N] __attribute__ ((aligned (64)));

// pointer aliasing
double *restrict rres;
double *restrict rmul1;
double *restrict rmul2;


void original() {
	for (i = 0; i < N; ++i)
		for (j = 0; j < N; ++j)
			for (k = 0; k < N; ++k)
				res[i][j] += mul1[i][k] * mul2[k][j];
}

void transposed() {
    for (i = 0; i < N; ++i)
        for (j = 0; j < N; ++j)
            tmp[i][j] = mul2[j][i];
	for (i = 0; i < N; ++i)
		for (j = 0; j < N; ++j)
			for (k = 0; k < N; ++k)
				res[i][j] += mul1[i][k] * tmp[j][k];
}

void sub_matrix() {
	for (i = 0; i < N; i += SM)
		for (j = 0; j < N; j += SM)
			for (k = 0; k < N; k += SM)
				for (i2 = 0, rres = &res[i][j], rmul1 = &mul1[i][k]; i2 < SM; ++i2, rres += N, rmul1 += N)
					for (k2 = 0, rmul2 = &mul2[k][j]; k2 < SM; ++k2, rmul2 += N)
						for (j2 = 0; j2 < SM; ++j2)
							rres[j2] += rmul1[k2] * rmul2[j2];
}

void vectorized() {
	for (i = 0; i < N; i += SM)
		for (j = 0; j < N; j += SM)
			for (k = 0; k < N; k += SM)
				for (i2 = 0, rres = &res[i][j], rmul1 = &mul1[i][k]; i2 < SM; ++i2, rres += N, rmul1 += N) {
					_mm_prefetch (&rmul1[8], _MM_HINT_NTA);
					for (k2 = 0, rmul2 = &mul2[k][j]; k2 < SM; ++k2, rmul2 += N) {
						__m128d m1d = _mm_load_sd (&rmul1[k2]);
						m1d = _mm_unpacklo_pd (m1d, m1d);
						for (j2 = 0; j2 < SM; j2 += 2) {
							__m128d m2 = _mm_load_pd (&rmul2[j2]);
							__m128d r2 = _mm_load_pd (&rres[j2]);
							_mm_store_pd (&rres[j2], _mm_add_pd (_mm_mul_pd (m2, m1d), r2));
						}
					}
				}
}


long long timeinterval_to_long(struct timeval start, struct timeval end)
{
	return (end.tv_sec - start.tv_sec) * 1000000LL + (end.tv_usec - start.tv_usec);
}

int main(int argc, char const *argv[])
{
	struct timeval startTime;
	struct timeval endTime;
	long long elapsedTime;
	long long originalTime;

	for (i = 0; i < N; ++i)
	{
		for (j = 0; j < N; ++j) 
		{
			mul1[i][j] = 0.0;
			mul2[i][j] = 0.0;
		}
	}
	gettimeofday(&startTime, 0);
	original();+
	gettimeofday(&endTime, 0);
	originalTime = timeinterval_to_long(startTime, endTime);
	printf("Original took: %lld ms\n", originalTime );

	gettimeofday(&startTime, 0);
	transposed();
	gettimeofday(&endTime, 0);
	elapsedTime = timeinterval_to_long(startTime, endTime);
	printf("Transposed took: %lld ms. %.2f%% of Original\n", elapsedTime, ((float)elapsedTime / (float)originalTime) * 100.f);

	gettimeofday(&startTime, 0);
	sub_matrix();
	gettimeofday(&endTime, 0);
	elapsedTime = timeinterval_to_long(startTime, endTime);
	printf("Sub-Matrix took: %lld ms. %.2f%% of Original\n", elapsedTime, ((float)elapsedTime / (float)originalTime) * 100.f);

	gettimeofday(&startTime, 0);
	vectorized();
	gettimeofday(&endTime, 0);
	elapsedTime = timeinterval_to_long(startTime, endTime);
	printf("Vectorized took: %lld ms. %.2f%% of Original\n", elapsedTime, ((float)elapsedTime / (float)originalTime) * 100.f);

	return 0;
}